/*
 * @Description: front end 任务管理， 放在类里，代码更清晰，已审核
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:05:15
 * @LastEditTime: 2021-10-27 14:05:05
 * @LastEditors: Sang Hao
 */

#include <ros/ros.h>
#include "glog/logging.h"

#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/mapping/front_end/front_end_flow.hpp"

namespace lidar_slam {
/**
 * @description: 前端任务管理构造函数
 * @param  {*}
 * @return {*}
 */
FrontEndFlow::FrontEndFlow(ros::NodeHandle& nh) {
	// 订阅同步过的点云synced_cloud
	cloud_sub_ptr_ = std::make_shared<CloudSubscriber>(nh, "/synced_cloud", 100000);
	// OdometryPublisher: topic_name，base_frame_id, child_frame_id	
	laser_odom_pub_ptr_ = std::make_shared<OdometryPublisher>(
		nh, "/laser_odom", "/map", "/lidar", 100);
	front_end_ptr_ = std::make_shared<FrontEnd>();
}

bool FrontEndFlow::Run() {
	if (!ReadData()) {
		return false;
	}
	while (HasData()) {
		if (!ValidData()) {
			continue;
		}
		if (UpdateLaserOdometry()) {
			PublishData();
		}
	}
	return true;
}

/**
 * @description: 将Cloud_sub_ptr_订阅到的数据放入cloud_data_buff_队列中去
 * @param  {*}
 * @return {*}
 */
bool FrontEndFlow::ReadData() {
	cloud_sub_ptr_->ParseData(cloud_data_buff_);
	return true;	
}

bool FrontEndFlow::HasData() {
	return cloud_data_buff_.size() > 0;
}

bool FrontEndFlow::ValidData() {
	current_cloud_data_ = cloud_data_buff_.front();
	cloud_data_buff_.pop_front();

	return true;
}

bool FrontEndFlow::UpdateLaserOdometry() {
	static bool init_laser_odometry = false;
	if (!init_laser_odometry) {
		init_laser_odometry = true;
		front_end_ptr_->SetInitPose(Eigen::Matrix4f::Identity());
		return front_end_ptr_->Update(current_cloud_data_, laser_odometry_);
	}

	return front_end_ptr_->Update(current_cloud_data_, laser_odometry_);
}

bool FrontEndFlow::PublishData() {
	laser_odom_pub_ptr_->Publish(laser_odometry_, current_cloud_data_.time);
	return true;
}
}